#ifndef KINEMATICS_DIFF_H_
#define KINEMATICS_DIFF_H_

//FIXME: remove ros
#include "rclcpp/rclcpp.hpp"
#include <chassis_controller/pivots.h>
#include "chassis_controller/kinematics.h"
#include <chassis_controller/speed_limiter.h>
namespace chassis_controller
{
class DiffKinematics : public Kinematics
{
public:
  explicit DiffKinematics(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node);

  void getCommand(double vx, double vy , double va);

  void getVelocity(double vx, double vy , double va);

  /// @brief 传入转舵和限速参数
  /// @param pivot_num  驱动单元数量
  /// @param steer_pid_param 转舵PID参数
  /// @param limiter_liner  速度限制
  /// @param limiter_steer  转舵速度限制
  void init(int pivot_num, pid_param steer_pid_param,SpeedLimiter limiter_liner, SpeedLimiter limiter_ang ,SpeedLimiter limiter_steer);

  /// @brief 传入转舵的动态参数
  void updateParam(int pivot_num, pid_param steer_pid_param);

  /// @brief 根据底盘参数、当前的个驱动单元的速度，计算各驱动电机的速度或位置
  void update(std::vector<Spivot_value> &pivot_value, 
              std::vector<Spivot_param> &pivot_param , 
              std::vector<Spivot_setpoint> &pivot_setpoint ,const rclcpp::Duration &period,
              bool ready_move);

  /// @brief 目标速度或为位置赋值到hardware interface 
  void setCommand(std::vector<Spivot_value> &pivot_value, 
                  std::vector<Spivot_param> &pivot_param , 
                  std::vector<Spivot_setpoint> &pivot_setpoint ,
                  const rclcpp::Duration &period) ;

  bool getSteeringFlag() ;

  bool getSteerErrFlag() ;
  bool getBrakerFlag();
  uint64_t getSteerCmdId() ;
  

  void setSteerCmdId( uint64_t steer_cmd_id ) ;


private:

  std::vector<double> liner_setpoint_;
  std::vector<double> angle_setpoint_;
  std::vector<double> angle_setpoint_last_valid;

  std::vector<SpeedLimiter> limiter_liner_;
  std::vector<double> last0_cmd_liner_;
  std::vector<double> last1_cmd_liner_;


  Sxya setpoint_;
  Sxya current_vel_;

  bool movement_;
  bool drive_request_;
  bool standstill_;
  bool braker_open_;

  int pivots_num_;
  double start_time_;

double normAngle2(double angle)
{
    double fRad = angle;
    // Scale the angle into [-PI, +)
    while(fRad < -PI)
        fRad += 2*PI;
    // Scale the angle into [-PI, PI)
    while (fRad >= PI)
        fRad -= 2*PI;
    return fRad;
}
};
}
#endif
